#include <libplayerc++/playerc++.h>
#include <joystick.h>
#include <kalman.h>

#define RED             0xff0000
#define BLUE            0x0000ff
#define GREEN           0x00ff00
#define CYAN            0x00ffff
#define YELLOW          0xffff00
#define MAGENTA         0xff00ff
#define LIGHTSTEELBLUE  0xb0c4de
#define SEAGREEN        0x2e8b57
#define GOLDENROD       0xdaa520
#define CHOCOLATE       0xd2691e
#define PEACHPUFF1      0xffdab9
#define OLIVEDRAB1      0xc0ff3e
#define GRAY            0x7f7f7f

#define MAX_RANGE   8.0

#define LEFT    ((360-(*bp)[blob_index].left))
#define RIGHT   ((360-(*bp)[blob_index].right))
#define START   (((RIGHT < LEFT) ? RIGHT : LEFT) > 0) ? ((RIGHT < LEFT) ? RIGHT : LEFT) : 0;
#define END     (((LEFT > RIGHT) ? LEFT : RIGHT) > 0) ? ((LEFT > RIGHT) ? LEFT : RIGHT) : 0;

#define REF_ANGLE   (double) (360-bp[i].x)/2

int countBlobs(PlayerCc::BlobfinderProxy *bp);
int getLandmarkID (int color);
double getLandmarkRange(PlayerCc::LaserProxy *lp, PlayerCc::BlobfinderProxy *bp, int blob_index);
double getLandmarkBearing(PlayerCc::LaserProxy *lp, PlayerCc::BlobfinderProxy *bp, int blob_index);
const char* getLandmarkSignature(PlayerCc::BlobfinderProxy *bp, int blob_index);
void updateEstimatedLandmarks(PlayerCc::SimulationProxy *sp, int *signature_blob, list<lista_mapa> *lista_mapa);
void updateEstimatedRobot(PlayerCc::SimulationProxy *sp, PGMATRIX Position);
